/*
 * pwmPacketHandler.h
 *
 * Created: 9/23/2013 3:23:29 PM
 *  Author: Tim
 */ 


#ifndef PWMPACKETHANDLER_H_
#define PWMPACKETHANDLER_H_

static inline status_t servoEnableHandler(servoEnable_t * packet)
{
			
	uint8_t errorCode = 0;
	
	errorCode |= packet->PARAM1.servoEn_msk0;
	errorCode |= packet->PARAM1.servoEn_msk1<<1;
	errorCode |= packet->PARAM1.servoEn_msk2<<2;
	errorCode |= packet->PARAM1.servoEn_msk3<<3;
		
	if(packet->PARAM1.servoEn_msk0)
	{
		//check to see if something else uses this pin already
		if(daqAlloc.dio[12].alloc == FEATURE_UTILIZED)	
		{
			sendGenAck(servoEnable_mh,0x1);
			errorCode &= ~(0x1);
		}
	}
	
	if(packet->PARAM1.servoEn_msk1)
	{
		//check to see if something else uses this pin already
		if(daqAlloc.dio[11].alloc == FEATURE_UTILIZED)
		{
			sendGenAck(servoEnable_mh,0x2);
			errorCode &= ~(0x2);
		}
	}
	
	if(packet->PARAM1.servoEn_msk2)
	{
		//check to see if something else uses this pin already
		if(daqAlloc.dio[10].alloc == FEATURE_UTILIZED)
		{
			sendGenAck(servoEnable_mh,0x4);
			errorCode &= ~(0x4);
		}
	}
	
	if(packet->PARAM1.servoEn_msk3)
	{
		//check to see if something else uses this pin already
		if(daqAlloc.dio[9].alloc == FEATURE_UTILIZED)
		{
			sendGenAck(servoEnable_mh,0x8);
			errorCode &= ~(0x8);
		}
	}
	
		// IF HERE THE MSG PASSED
		// NOW ALL THE PARAMS ARE SET
		
		if(errorCode & 0x1)
		{
			//since we enabled a servo we know that the actuator packet will be 2 bytes longer now 
			actuatorSuperPacketLength += 2;
			daqAlloc.actuatorAlloc.servo0Alloc = FEATURE_UTILIZED;
			daqAlloc.dio[12].alloc = FEATURE_UTILIZED;
			daqAlloc.dio[12].source = DIO_PWM;
			dioDirSet(12);
			TCE0.CTRLB |= TC0_CCAEN_bm;
		}

		if(errorCode & 0x2)
		{
			//since we enabled a servo we know that the actuator packet will be 2 bytes longer now
			actuatorSuperPacketLength += 2;
			daqAlloc.actuatorAlloc.servo1Alloc = FEATURE_UTILIZED;
			daqAlloc.dio[11].alloc = FEATURE_UTILIZED;
			daqAlloc.dio[11].source = DIO_PWM;
			dioDirSet(11);
			TCE0.CTRLB |= TC0_CCBEN_bm;
		}

		if(errorCode & 0x4)
		{
			//since we enabled a servo we know that the actuator packet will be 2 bytes longer now
			actuatorSuperPacketLength += 2;
			daqAlloc.actuatorAlloc.servo2Alloc = FEATURE_UTILIZED;
			daqAlloc.dio[10].alloc = FEATURE_UTILIZED;
			daqAlloc.dio[10].source = DIO_PWM;
			dioDirSet(10);
			TCE0.CTRLB |= TC0_CCCEN_bm;
		}

		if(errorCode & 0x8)
		{
			//since we enabled a servo we know that the actuator packet will be 2 bytes longer now
			actuatorSuperPacketLength += 2;
			daqAlloc.actuatorAlloc.servo3Alloc = FEATURE_UTILIZED;
			daqAlloc.dio[9].alloc = FEATURE_UTILIZED;
			daqAlloc.dio[9].source = DIO_PWM;
			dioDirSet(9);
			TCE0.CTRLB |= TC0_CCAEN_bm;
		}
		
			//if the message passed then we definitely will be using timer E0
			daqAlloc.timerAlloc.timerE0Alloc = FEATURE_UTILIZED;
			
			//acknowledge the host and specify what was set for good measure
			sendGenAck(servoEnable_mh,0x80 | errorCode);
			return OPP_SUCCESSFUL;
	
}

static inline status_t motorEnableHandler(motorEnable_t * packet)
{
uint8_t errorCode = 0;
uint8_t failed = 0;
errorCode |= packet->PARAM1.motorEn_msk0;
errorCode |= packet->PARAM1.motorEn_msk1<<1;
errorCode |= packet->PARAM1.motorEn_msk2<<2;
errorCode |= packet->PARAM1.motorEn_msk3<<3;

if(packet->PARAM1.motorEn_msk0)
{
	//check to see if something else uses this pin already
	if(daqAlloc.dio[1].alloc == FEATURE_UTILIZED)
	{
		sendGenAck(motorEnable_mh,0x1);
		errorCode &= ~(0x1);
	}
}

if(packet->PARAM1.motorEn_msk1)
{
	//check to see if something else uses this pin already
	if(daqAlloc.dio[2].alloc == FEATURE_UTILIZED)
	{
		sendGenAck(motorEnable_mh,0x2);
		errorCode &= ~(0x2);
	}
}

if(packet->PARAM1.motorEn_msk2)
{
	//check to see if something else uses this pin already
	if(daqAlloc.dio[3].alloc == FEATURE_UTILIZED)
	{
		sendGenAck(motorEnable_mh,0x4);
		errorCode &= ~(0x4);
	}
}

if(packet->PARAM1.motorEn_msk3)
{
	//check to see if something else uses this pin already
	if(daqAlloc.dio[4].alloc == FEATURE_UTILIZED)
	{
		sendGenAck(motorEnable_mh,0x8);
		errorCode &= ~(0x8);
	}
}


// IF HERE THE MSG PASSED
// NOW ALL THE PARAMS ARE SET

if(errorCode & 0x1)
{
	//since we enabled a motor we know that the actuator packet will be 2 bytes longer now
	actuatorSuperPacketLength += 2;
	daqAlloc.actuatorAlloc.motor0Alloc = FEATURE_UTILIZED;
	daqAlloc.dio[1].alloc = FEATURE_UTILIZED;
	daqAlloc.dio[1].source = DIO_PWM;
	dioDirSet(1);
	TCD0.CTRLB |= TC0_CCAEN_bm;
}

if(errorCode & 0x2)
{
	//since we enabled a motor we know that the actuator packet will be 2 bytes longer now
	actuatorSuperPacketLength += 2;
	daqAlloc.actuatorAlloc.motor1Alloc = FEATURE_UTILIZED;
	daqAlloc.dio[2].alloc = FEATURE_UTILIZED;
	daqAlloc.dio[2].source = DIO_PWM;
	dioDirSet(2);
	TCD0.CTRLB |= TC0_CCBEN_bm;
}

if(errorCode & 0x4)
{
	//since we enabled a motor we know that the actuator packet will be 2 bytes longer now
	actuatorSuperPacketLength += 2;
	daqAlloc.actuatorAlloc.motor2Alloc = FEATURE_UTILIZED;
	daqAlloc.dio[3].alloc = FEATURE_UTILIZED;
	daqAlloc.dio[3].source = DIO_PWM;
	dioDirSet(3);
	TCD0.CTRLB |= TC0_CCCEN_bm;
}

if(errorCode & 0x8)
{
	//since we enabled a motor we know that the actuator packet will be 2 bytes longer now
	actuatorSuperPacketLength += 2;
	daqAlloc.actuatorAlloc.motor3Alloc = FEATURE_UTILIZED;
	daqAlloc.dio[4].alloc = FEATURE_UTILIZED;
	daqAlloc.dio[4].source = DIO_PWM;
	dioDirSet(4);
	TCD0.CTRLB |= TC0_CCDEN_bm;
}

//if the message passed then we definitely will be using timer E0
daqAlloc.timerAlloc.timerE0Alloc = FEATURE_UTILIZED;

//acknowledge the host and specify what was set for good measure
sendGenAck(motorEnable_mh,0x80 | errorCode);
return OPP_SUCCESSFUL;
}

#endif /* PWMPACKETHANDLER_H_ */